
using System.Collections.Generic;
using System;

namespace cs5643.rigidbody
{



    /**
     * Class to detect and resolve collisions (e.g., using penalty forces)
     * between rigid bodies. Implement both broad and narrow phase checks
     * here.
     * 
     * @author Doug James, March 2007.
     */
    public class CollisionProcessor
    {
        private RigidBody[] B;
        private HashSet<BodyPair> candidateBodyPairs = new HashSet<BodyPair>();

        /**
         * Index bodies, and BUILD YOUR FAVORITE DATA STRUCTURES!
         */
        public CollisionProcessor(HashSet<RigidBody> bodies)
        {
            B = new RigidBody[bodies.Count];
            bodies.CopyTo(B);

            /// INDEX BODIES (could use hash to avoid storing info in RigidBody):
            for (int k = 0; k < B.Length; k++)
            {
                B[k].setKey(k);
            }
        }

            /// TODO: BUILD YOUR DATASTRUCTURES
            /// ... 
            /// ...
            /// 

                public double[] gaussSeidel(List<Pair<int, Double>>[] a, double[] b) {
    	double epsilon = 0.01;
    	double error = 1.0;
    	double[] x = new double[b.Length];
    	double xlo = Double.MinValue;
    	double xhi = Double.MaxValue;
    	while (error > epsilon) {
    		for (int i = 0 ; i < b.Length ; i++) {
    			double update = b[i];
    			List<Pair<int, Double>> row = a[i];
    			foreach (Pair<int, Double> val in row) {
    				if (val.first < i) {
    					update -= val.second*x[val.first];
    				} else if (val.first > i) {
    					update -= val.second*x[val.first];
    				} else {
    					update /= val.second;
    				}
    			}
    			x[i] = update;
    			int r = i%2;
    			if (r == 1) {
    				xhi = Constants.FRICTION_COEFFICIENT*x[i-1];
    				xlo = -xhi;
        			x[i] = Math.Min(Math.Max(xlo, x[i]), xhi);
    			}
    		}
    		error = 0.0;
    		double length = 0.0;
    		for (int i = 0; i < b.Length; i++) {
    			length += b[i]*b[i];
    			double temp = 0.0;
    			foreach (Pair<int, Double> val in a[i]) {
    				temp += val.second*x[val.first];
    			}
    			error += (b[i] - temp)*(b[i] - temp);
    		}
    		error = Math.Sqrt(error/length);
    	}
    	return x;
    

        }

        /**
         * Performs broad and narrow phase collision detection, and
         * applies penalty forces at contacts.
         */
        public void processCollisions()
        {
            /// BROAD PHASE:
            broadPhase();// --> updates candidateBodyPairs

            /// NARROW PHASE: Detect collisions and apply forces
            narrowPhase();
        }

        /** Insert your implementation here of whatever broad phase test
         * you choose. */
        void broadPhase()
        {
            /// TEMPORARY: Just do all Disk-Disk pairs (ha ha ha!)
            candidateBodyPairs.Clear();
            for (int i = 0; i < B.Length; i++)
            {
                for (int j = 0; j < i; j++)
                {
                    if (B[i].intersectsBounds(B[j]))
                    {///DISK TEST (NOT VERY ACCURATE), 
                        candidateBodyPairs.Add(new BodyPair(i, j));
                    }
                }
            }
            //Console.Out.WriteLine("BROAD PHASE: |candidateBodyPairs|="+candidateBodyPairs.Count);
        }

        /** Insert your implementation of narrow phase collision detection
         * and penalty force response here. */
        void narrowPhase()
        {
            /// TEMPORARY: All pairs... (ha ha ha)
            foreach (BodyPair pair in candidateBodyPairs)
                processBodyPair(pair);
        }

        /** Narrow phase BodyPair collision resolution. You'll want to
         * borrow ideas from this function for your narrow phase code.  */
        void processBodyPair(BodyPair pair)
        {
            int i = pair.i;
            int j = pair.j;
            if (i == j) return;

            /// USE INTELLIGENT ALL-PAIRS TEST: 
            /// 
            /// TEMP/HACK... just use all pairs (ha ha ha):
            RigidBody bodyI = B[i];
            RigidBody bodyJ = B[j];
            double massEff = 1.0 / (1.0 / bodyI.getMass() + 1.0 / bodyJ.getMass());
            // 	Collection<Block> blocksI = bodyI.getBlocks();/// SLOTH: ONLY BOUNDARY BLOCKS NEEDED
            // 	Collection<Block> blocksJ = bodyJ.getBlocks();/// SLOTH: ONLY BOUNDARY BLOCKS NEEDED
            ICollection<Block> blocksI = bodyI.getBoundaryBlocks();
            ICollection<Block> blocksJ = bodyJ.getBoundaryBlocks();
            Pnt2D piW = new Pnt2D(0, 0);
            Pnt2D pjW = new Pnt2D(0, 0);
            Pnt2D v = new Pnt2D(0, 0);
            foreach (Block bi in blocksI)
            {

                piW.Set(bi.p) ;//body coords
                bodyI.do_transformB2W(ref piW);//-->world

                foreach (Block bj in blocksJ)
                {

                    /// SLOTH (REPEATED ~O(MN) times)
                    pjW.Set(bj.p);//body coords
                    bodyJ.do_transformB2W(ref pjW);//-->world

                    ///TEST bi-bj AND APPLY WRENCH IF OVERLAPPING:
                    v = Pnt2D.Sub(pjW,piW);
                    double dist = v.Length + Constants.eps10;
                    double sumRadii = 1.41 * (bi.h + bj.h);
                    double penDepth = sumRadii - dist;// >0

                    /// APPLY PENALTY FORCE IF IN CONTACT:
                    if (penDepth > 0)
                    {//overlap
                        v.Normalize();

                        /// PENALTY CONTACT FORCE:
                        double k = Constants.CONTACT_STIFFNESS * massEff;
                        double force = k * penDepth;

                        /// DAMPING: 
                        double vi = v.Dot(bodyI.getSpatialVelocityW(piW));
                        double vj = v.Dot(bodyJ.getSpatialVelocityW(pjW));
                        double fDamp = +0.004 * k * (vi - vj);

                        v .Times (force + fDamp);
                        bodyJ.applyContactForceW(pjW, v);
                        v .Times( -1);
                        bodyI.applyContactForceW(piW, v);
                    }
                }
            }
        }
    }
}